/**
 * @file   can_uds.c
 * @brief  CAN UDS相关函数，包含UDS服务处理部分代码
 * @author wdluo(wdluo@toomoss.com)
 * @version 1.0
 * @date 2022-10-18
 * @copyright Copyright (c) 2022 重庆图莫斯电子科技有限公司
 */
#include "system_init.h"
#include <stdint.h>
#include <stdio.h>
#include "can_uds.h"
#include "crc32.h"
#include "can_uds.h"
#include "can_boot.h"
#include "can_buffer.h"
/**
 * @brief  循环从FIFO读取1帧数据
 * @param pCANData 数据指针
 * @return 成功读取到的数据字节数
 */
uint8_t CAN_UDS_GetMsg(uint8_t *pCANData)
{
  CAN_MSG *pMsg;
  if(CAN_BUF_PopMsg(0,&pMsg,1)){
    if(pMsg->Flags.DIR){
      CAN_WriteData(0,pMsg);
    }else{
      memcpy(pCANData,pMsg->Data,pMsg->Flags.DLC);
      return pMsg->Flags.DLC;
    }
  }
  return 0;
}

/**
 * @brief  等待流控帧
 * @param[out] pUDSFC 流控帧帧头指针
 * @param TimeOut 超时时间，单位为毫秒
 * @return 等待流控帧状态
 * @retval 1 成功获取到流控帧
 * @retval 0 未获取到流控帧
 */
uint8_t CAN_UDS_WaitFC(UDS_FC_HEAD *pUDSFC,int TimeOut)
{
    do{
        uint8_t buffer[64];
        int DataIndex = 0;
        int DataLen = CAN_UDS_GetMsg(buffer);
        if(((UDS_ADDR_FORMATS==NORMAL)&&(DataLen >= 3))||((UDS_ADDR_FORMATS!=NORMAL)&&(DataLen >= 4))){
            DataIndex = 0;
            if(UDS_ADDR_FORMATS!=NORMAL){
                uint8_t AddrExt = buffer[DataIndex++];
            }
            uint8_t PCI = buffer[DataIndex++];
            pUDSFC->PCIBits.FC_FS = PCI&0xF;
            pUDSFC->PCIBits.PCIType = (PCI>>4)&0xF;
            pUDSFC->FC_BS = buffer[DataIndex++];
            pUDSFC->FC_STmin = buffer[DataIndex++];
            if((pUDSFC->PCIBits.PCIType==UDS_FC)&&(pUDSFC->PCIBits.FC_FS < 3)){
                if(pUDSFC->PCIBits.FC_FS==0){
                    return 1;
                }else if(pUDSFC->PCIBits.FC_FS==2){
                    return 0;
                }
            }
        }
        systick_delay_ms(1);
        TimeOut--;
    }while(TimeOut>0);
    return 0;
}
/**
 * @brief  UDS 服务响应处理
 * @param  RSID 响应ID
 * @param  pData 响应数据指针
 * @param  DataLen 响应数据字节数
 * @return 响应状态
 * @retval 1 成功
 * @retval 0 失败
 */
uint8_t CAN_UDS_ServiceRespond(uint8_t RSID,uint8_t *pData,uint32_t DataLen)
{
#if DEBUG_SHOW_UDS_DATA
    printf("UDS TX:%02X ",RSID);
    for(int i=0;i<DataLen;i++){
        printf("%02X ",pData[i]);
    }
    printf("\r\n");
#endif
    uint8_t buffer[64];
    int MaxSFDataLen=0;
    int MaxFFDataLen=0;
    int MaxCFDataLen=0;
    int DataIndex=0;
    if(UDS_ADDR_FORMATS==NORMAL){
        MaxSFDataLen = UDS_MAX_DLC-1;
        MaxFFDataLen = UDS_MAX_DLC-2;
        MaxCFDataLen = UDS_MAX_DLC-1;
    }else{
        MaxSFDataLen = UDS_MAX_DLC-2;
        MaxFFDataLen = UDS_MAX_DLC-3;
        MaxCFDataLen = UDS_MAX_DLC-2;
        buffer[DataIndex++] = UDS_EXT_ADDR;
    }
    if((DataLen+1) <= MaxSFDataLen){//SF
        buffer[DataIndex++] = DataLen+1;//PCI
        buffer[DataIndex++] = RSID;
        for(int i=0;i<DataLen;i++){
            buffer[DataIndex++] = *pData++;
        }
        uint8_t ret = CAN_UDS_SendData(buffer,DataIndex);
        if(!ret){
            return ret;
        }
    }else{//FF+CF
        UDS_FC_HEAD UDSFC;
        //FF
        buffer[DataIndex++] = (UDS_FF<<4)|(((DataLen+1)>>8)&0xF);//PCI
        buffer[DataIndex++] = (DataLen+1)&0xFF;
        buffer[DataIndex++] = RSID;
        for(int i=0;i<(MaxFFDataLen-1);i++){
            buffer[DataIndex++] = *pData++;
        }
        uint8_t ret = CAN_UDS_SendData(buffer,DataIndex);
        if(!ret){
            return ret;
        }
        DataLen -= (MaxFFDataLen-1);
        //等待FC
        ret = CAN_UDS_WaitFC(&UDSFC,1000);
        if(!ret){
            return ret;
        }
        //发送CF
        int di=0;
        int CFNum=0;
        uint8_t CF_SN=1;
        for(di=0;di<DataLen;){
            int SendDataNum = ((di+MaxCFDataLen)>DataLen)?(DataLen%MaxCFDataLen):MaxCFDataLen;
            if(SendDataNum > 0){
                DataIndex = 0;
                if(UDS_ADDR_FORMATS!=NORMAL){
                    buffer[DataIndex++] = UDS_EXT_ADDR;
                }
                buffer[DataIndex++] = (UDS_CF<<4)|(CF_SN&0xF);//PCI
                for(int i=0;i<SendDataNum;i++){
                    buffer[DataIndex++] = *pData++;
                }
                ret = CAN_UDS_SendData(buffer,DataIndex);
                if(!ret){
                    return ret;
                }
                CF_SN++;
                CFNum++;
                di += SendDataNum;
                if((UDSFC.FC_STmin>=1)&&(UDSFC.FC_STmin<=127)){
                	systick_delay_ms(UDSFC.FC_STmin);
                }
                //再次计算下次发送字节数，看是否大于0
                if((UDSFC.FC_BS > 0)&&(di < DataLen)){
                    if(CFNum == UDSFC.FC_BS){
                        //等待FC
                        ret = CAN_UDS_WaitFC(&UDSFC,1000);
                        if(!ret){
                            return ret;
                        }
                        CFNum = 0;
                    }
                }
            }
        }
    }
    return 1;
}
/**
 * @brief  处理UDS服务，UDS相关功能在此函数实现
 * @param  SID 服务ID
 * @param  pData 服务参数指针
 * @param  DataLen 服务参数长度
 * @return 函数执行状态
 * @retval 1 成功
 * @retval 0 失败
 */
uint8_t CAN_UDS_ServiceProcess(uint8_t SID,uint8_t *pData,uint32_t DataLen)
{
  static uint32_t MemoryAddress;
  static uint32_t MemorySize;
  static uint32_t AppCRC32=0xFFFFFFFF;
  //static uint8_t DataBuffer[1024+2];
  static uint8_t FirstCRC=1;
  const uint32_t BlockSize = UDS_PACK_SIZE+2;//Max Number Of Block Length,需要加上SID和BSC
  static uint8_t BSC=1;//Block Sequence Counter
  switch(SID)
  {
    case 0x10:
      if((DataLen>=1)&&(pData[0]==0x03)){//extendedDiagnosticSession
        printf("Extended Diagnostic Session\r\n");
        CAN_UDS_ServiceRespond(SID|0x40,pData,1);
      }
      else if((DataLen>=1)&&(pData[0]==0x02)){//Programming Mode
        AppCRC32=0xFFFFFFFF;
        FirstCRC = 1;
        printf("Programming Mode\r\n");
        CAN_UDS_ServiceRespond(SID|0x40,pData,1);
        CAN_BOOT_SetProgRequest();
        printf("ECU Reset\r\n");
        CAN_BOOT_Reset();
        
      }
      break;
    case 0x11:
      if((DataLen>=1)&&(pData[0]==0x01)){//ECU Reset
        printf("ECU Reset\r\n");
        CAN_UDS_ServiceRespond(SID|0x40,pData,1);
        CAN_BOOT_ResetProgRequest();//清除进入编程模式标志
        CAN_BOOT_Reset();
      }
      break;
    case 0x31:
      if((DataLen>=3)&&(pData[0]==0x01)&&(pData[1]==0xFF)&&(pData[2]==0x00)){//Erase Flash Memory
        printf("Erase Flash Memory...\r\n");
        uint8_t res[2]={SID,0x78};//Request Correctly Received Response Pending
        CAN_UDS_ServiceRespond(0x7F,res,2);
        pData[3] = CAN_BOOT_EraseApp();
        CAN_UDS_ServiceRespond(SID|0x40,pData,4);
      }else if((DataLen>=3)&&(pData[0]==0x01)&&(pData[1]==0xFF)&&(pData[2]==0x01)){//Check Programming Dependencies,验证APP的合法性
        pData[3] = CAN_BOOT_CheckApp();
        CAN_UDS_ServiceRespond(SID|0x40,pData,4);
        printf("Check Programming Dependencies res=%d\r\n",pData[3]);
      }else if((DataLen>=7)&&(pData[0]==0x01)&&(pData[1]==0x02)&&(pData[2]==0x02)){//Check Programming Integrity,通过CRC32验证
        uint32_t CheckSum = (pData[3]<<24)|(pData[4]<<16)|(pData[5]<<8)|pData[6];
        if(AppCRC32==CheckSum){
          pData[3] = 0;//校验成功
          CAN_BOOT_SetAppValid();
        }else{
          pData[3] = 1;//校验失败
        }
        CAN_UDS_ServiceRespond(SID|0x40,pData,4);
        printf("Check Programming Integrity GetCRC=%08X AppCRC=%08X\r\n",CheckSum,AppCRC32);
        //复位CRC，下次开始计算的时候就不会出错
        AppCRC32=0xFFFFFFFF;
        FirstCRC = 1;
      }else if((DataLen>=3)&&(pData[0]==0x01)&&(pData[1]==0x02)&&(pData[2]==0x03)){//Check Programming Dependencies,验证APP的合法性
        pData[3] = 0;//状态值
        CAN_UDS_ServiceRespond(SID|0x40,pData,4);
        printf("Check Programming Dependencies res=%d\r\n",pData[3]);
      }else if((DataLen>=1)&&(pData[0]==0x01)){
        CAN_UDS_ServiceRespond(SID|0x40,pData,1);
      }else{
        pData[0] = SID;
        pData[1] = 0x73;//Wrong Block Sequence Counter
        CAN_UDS_ServiceRespond(0x7F,pData,2);
      }
      break;
    case 0x34:
      if((DataLen>=10)&&(pData[0]==0x00)&&(pData[1]==0x44)){//Request Download
        BSC = 1;
        MemoryAddress = (pData[2]<<24)|(pData[3]<<16)|(pData[4]<<8)|pData[5];
        MemorySize = (pData[6]<<24)|(pData[7]<<16)|(pData[8]<<8)|pData[9];
        printf("Request Download MemoryAddress = 0x%08X MemorySize = %d Byte\r\n",MemoryAddress,MemorySize);
        pData[0] = 0x40;
        pData[1] = BlockSize>>24;
        pData[2] = BlockSize>>16;
        pData[3] = BlockSize>>8;
        pData[4] = BlockSize;
        CAN_UDS_ServiceRespond(SID|0x40,pData,5);
      }else if((DataLen>=6)&&(pData[0]==0x00)&&(pData[1]==0x22)){//Request Download
        BSC = 1;
        MemoryAddress = (pData[2]<<8)|(pData[3]<<0);
        MemorySize = (pData[4]<<8)|(pData[5]<<0);
        printf("Request Download MemoryAddress = 0x%08X MemorySize = %d Byte\r\n",MemoryAddress,MemorySize);
        pData[0] = 0x20;
        pData[1] = BlockSize>>8;
        pData[2] = BlockSize;
        CAN_UDS_ServiceRespond(SID|0x40,pData,3);
      }else{
        pData[0] = SID;
        pData[1] = 0x13;
        CAN_UDS_ServiceRespond(0x7F,pData,2);
      }
      break;
    case 0x36:
      if(DataLen>=2){//Transfer Data
        //printf("Transfer Data BSC=%d\r\n",BSC);
        if(pData[0]==BSC){
          uint8_t res[2]={SID,0x78};//Request Correctly Received Response Pending
          CAN_UDS_ServiceRespond(0x7F,res,2);
          uint16_t DataNum = DataLen-1;
          uint8_t *pGotData = &pData[1];
          uint32_t AddrOffset = (BSC-1)*UDS_PACK_SIZE;
          CAN_BOOT_WriteDataToFlash(MemoryAddress+AddrOffset,pGotData,DataNum);
          CAN_UDS_ServiceRespond(SID|0x40,pData,1);
          //计算实际写入Flash中的数据CRC值
          if(FirstCRC){
            //AppCRC32 = crc32(AppCRC32,(const uint8_t *)pGotData,DataNum);
            AppCRC32 = crc32(AppCRC32,(const uint8_t *)(MemoryAddress+AddrOffset),DataNum);
            FirstCRC = 0;
          }else{
            //AppCRC32 = crc32(AppCRC32^0xFFFFFFFF,(const uint8_t *)pGotData,DataNum);
            AppCRC32 = crc32(AppCRC32^0xFFFFFFFF,(const uint8_t *)(MemoryAddress+AddrOffset),DataNum);
          }
          //printf("Transfer Data BSC=%d AppCRC=%08X\r\n",BSC,AppCRC32);
          printf("Write Data To Flash Addr=%08X\r\n",MemoryAddress+AddrOffset);
          //printf("AppCRC=%08X\r\n",AppCRC32);
          BSC++;
        }else{
          pData[0] = SID;
          pData[1] = 0x73;//Wrong Block Sequence Counter
          CAN_UDS_ServiceRespond(0x7F,pData,2);
          printf("Transfer Data BSC=%d\r\n",BSC);
        }
      }else{
        pData[0] = SID;
        pData[1] = 0x13;//Incorrect Message Length Or Invalid Format
        CAN_UDS_ServiceRespond(0x7F,pData,2);
      }
      break;
    case 0x85://ControlDTCSetting (0x85) service
      if(DataLen>=1){
        printf("ControlDTCSetting\r\n");
        CAN_UDS_ServiceRespond(SID|0x40,pData,DataLen);
      }
      break;
    case 0x28://CommunicationControl (0x28) service
      if((DataLen>=1)&&(pData[0]==0x03)){//禁止非诊断报文的发送和接收
        printf("CommunicationControl\r\n");
        CAN_UDS_ServiceRespond(SID|0x40,pData,DataLen);
      }
      break;
    case 0x27://SecurityAccess (0x27) service
      printf("SecurityAccess\r\n");
      if(pData[0]==0x01){
        pData[1] = 0x11;pData[2] = 0x22;pData[3] = 0x33;pData[4] = 0x44;//返回种子，可根据实际情况修改
        CAN_UDS_ServiceRespond(SID|0x40,pData,5);
      }else if(pData[0]==0x02){
        if(((pData[1]^0x11)==0xFF)&&((pData[2]^0x22)==0xFF)&&((pData[3]^0x33)==0xFF)&&((pData[4]^0x44)==0xFF)){//判断秘钥是否正确
          pData[1] = 0;
        }else{
          pData[1] = 1;
        }
        CAN_UDS_ServiceRespond(SID|0x40,pData,2);
      }
      break;
    case 0x37://Request Transfer Exit
      printf("Request Transfer Exit\r\n");
      CAN_UDS_ServiceRespond(SID|0x40,pData,0);
      break;
    default:
      break;
  }
  return 1;
}

/**
 * @brief  将要发送的CAN消息放入FIFO缓冲区
 * @param  pCANData 数据指针
 * @param  DataLen 数据有效字节数
 * @return 发送状态
 * @retval 1 成功
 * @retval 0 失败
  */
uint8_t CAN_UDS_SendData(uint8_t *pCANData,uint8_t DataLen)
{
    //调用CAN驱动层函数发送CAN数据
    CAN_MSG *pMsg = CAN_BUF_PickMsg(0);
    pMsg->Flags.DIR = 1;
    pMsg->Flags.RTR = 0;
    pMsg->ID = RSP_ADDR_ID;
    if(MSG_ID_TYPE==0){
      pMsg->Flags.IDE = 0;
    }else{
      pMsg->Flags.IDE = 1;
    }
    pMsg->Flags.DLC = 8;
    memset(pMsg->Data,0xAA,8);
    memcpy(pMsg->Data,pCANData,(DataLen>8)?8:DataLen);
    return CAN_WriteData(0,pMsg);
}

/**
  * @brief  处理CAN接收到的数据，处理之前要先判断接收帧为UDS帧
  * @param  pCANData CAN数据指针
  * @param  DataLen CAN数据有效字节数
  * @retval 无
  */
void CAN_UDS_Process(uint8_t *pCANData,uint8_t DataLen)
{
    int MaxSFDataLen=0;
    int MaxFFDataLen=0;
    int MaxCFDataLen=0;
    static int AllDataLen = 0;
    static int GotDataNum = 0;
    static uint8_t CF_SN=1;
    if(UDS_ADDR_FORMATS==NORMAL){
        MaxSFDataLen = UDS_MAX_DLC-1;
        MaxFFDataLen = UDS_MAX_DLC-2;
        MaxCFDataLen = UDS_MAX_DLC-1;
    }else{
        MaxSFDataLen = UDS_MAX_DLC-2;
        MaxFFDataLen = UDS_MAX_DLC-3;
        MaxCFDataLen = UDS_MAX_DLC-2;
    }
    static uint32_t buffer[UDS_PACK_SIZE/4+2];
    uint8_t *pBuffer = (uint8_t *)(&buffer[0])+2;
    uint8_t ResBuf[8];
    int DataIndex = 0;
    if(((UDS_ADDR_FORMATS==NORMAL)&&(DataLen >= 2))||((UDS_ADDR_FORMATS!=NORMAL)&&(DataLen >= 3))){
        if(UDS_ADDR_FORMATS!=NORMAL){
            uint8_t AddrExt = *pCANData++;
        }
        uint8_t PCI = *pCANData++;
        uint8_t PCIType = (PCI>>4)&0xF;
        switch(PCIType){
        case UDS_SF:
            GotDataNum = 0;
            AllDataLen = PCI&0xF;
            if(AllDataLen <= MaxSFDataLen){
                for(int i=0;i<AllDataLen;i++){
                	pBuffer[GotDataNum++] = *pCANData++;
                }
                //单帧接收完毕，可以在此处处理数据
#if DEBUG_SHOW_UDS_DATA
                printf("UDS RX:");
                for(int i=0;i<GotDataNum;i++){
                    printf("%02X ",buffer[i]);
                }
                printf("\r\n");
#endif
                CAN_UDS_ServiceProcess(pBuffer[0],&pBuffer[1],GotDataNum-1);
            }
            break;
        case UDS_FF:
            GotDataNum = 0;
            AllDataLen = ((PCI&0xF)<<8)|(*pCANData++);
            for(int i=0;i<MaxFFDataLen;i++){
            	pBuffer[GotDataNum++] = *pCANData++;
            }
            CF_SN = 1;
            //发送FC
            DataIndex = 0;
            if(UDS_ADDR_FORMATS!=NORMAL){
                ResBuf[DataIndex++] = UDS_EXT_ADDR;
            }
            ResBuf[DataIndex++] = (UDS_FC<<4)&0xF0;//PCI
            ResBuf[DataIndex++] = 0x00;//BS
            ResBuf[DataIndex++] = 0x00;//STmin,帧间隔时间，若单片机处理速度慢，建议设置该值大于0
            //调用发送CAN数据函数回复流控帧
            CAN_UDS_SendData(ResBuf,DataIndex);
#if DEBUG_SHOW_UDS_DATA
            printf("UDS TX:");
            for(int i=0;i<DataIndex;i++){
                printf("%02X ",ResBuf[i]);
            }
            printf("\r\n");
#endif
            break;
        case UDS_CF:
            if((PCI&0xF)==(CF_SN&0xF)){
                for(int i=0;(i<MaxCFDataLen)&&(GotDataNum<AllDataLen);i++){
                	pBuffer[GotDataNum++] = *pCANData++;
                }
                CF_SN++;
                if(GotDataNum>=AllDataLen){
                    //连续帧已经接收完毕数据，可以进行数据处理了
#if DEBUG_SHOW_UDS_DATA
                    printf("UDS RX:");
                    for(int i=0;i<GotDataNum;i++){
                        printf("%02X ",buffer[i]);
                    }
                    printf("\r\n");
#endif
                    CAN_UDS_ServiceProcess(pBuffer[0],&pBuffer[1],GotDataNum-1);
                }
            }
            break;
        case UDS_FC:
            break;
        default:
            break;
        }
    }
}
